LE:
April 4th, 2013 MM - strated coding the base model.
April 17th, 2013 MM - model can walk. derived model with "constant leg force" which also can walk.
April 18th, 2013 MM - derived model with lateral angle of attack w.r.t. velocity vector - can walk in circles.
April 23th, 2013 MM - added final triggers and "store" / "load" methods. Started work on analysis tools + viszualization.
April 29th, 2013 MM - moved class definitions to SVN
July 10th, 2013 MM - renamed and restructured notebook. Tested "unstable" walking pattern - created animation
Step 1: run the simulation
Step 2: create the animation
Step 3: show the animation
TODO:
add a possibility to inject energy.
Approach: Add two additional events:
At every event, a function updateparams(ss|ds) is called.
This notebooks aims at implementing the bipedal SLIP model for walking.
Use of this model as running SLIP is not intended because of different Poincare-Sections.
When finalized, a module will be composed out of this notebook for further use in other notebooks.
This model consists of two massless legs, attached to a point mass.
The Poincare-Section is defined as take-off of the trailing leg
Model parameters have the following structure:
param
.foot1 (1x3 array) location of foot 1
.foot2 (1x3 array) location of foot 2
.m (float) mass
.g (1x3 array) vector of gravity
.lp1 (3x float) list of leg parameters for leg 1
for SLIP: [l0, k, alpha, beta] (may be overwritten in derived models)
.lp2 (3x float) list of leg parameters for leg 2
This model can walk stably in 3D when lateral leg placement angles are antisymmetric.
It can also walk in circles when there is a difference in the magnitude of angles.
Also, asymmetric leg configurations do work (see example configuration).
In [26]:
from models import bslip
from pylab import pi
p = { 'foot1' : [0, 0, 0],
'foot2' : [-.5, 0, 0],
'm' : 80,
'g' : [0, -9.81, 0],
'lp1' : [13100, 1, 68.5 * pi / 180, -0.05], # leg params: stiffness, l0, alpha, beta
'lp2' : [12900, 1, 68.5 * pi / 180, 0.1],
}
IC = [0, .98, 0, 1.1, 0.01, 0.] # x,y,z, x',y',z'
# reload(bslip)
c = bslip.BSLIP_newTD(p, IC)
In [27]:
# run the model as often as you want (repeat this cell!)
for rep in range(30):
_ = c.do_step()
In [28]:
_ = bslip.vis_sim(c)
In [29]:
figure(figsize=(16,4))
tfin = 0
for ts, td, fs, fd in zip(c.t_ss_seq, c.t_ds_seq, c.forces_ss_seq, c.forces_ds_seq):
plot(ts + tfin,fs[:, 0],'r--')
plot(ts + tfin,fs[:, 1],'g--')
plot(ts + tfin,fs[:, 2],'b--')
plot(td + tfin,fd[:, 0],'r.-')
plot(td + tfin,fd[:, 1],'g.-')
plot(td + tfin,fd[:, 2],'b.-')
title('forces of leg 1')
xlabel('time [s]')
ylabel('force [N]')
Out[29]:
In [30]:
# provide a set of parameters
from pylab import pi
p = { 'foot1' : [0, 0, 0],
'foot2' : [-.5, 0, 0],
'm' : 80,
'g' : [0, -9.81, 0],
'lp1' : [13000, 1, 65.5 * pi / 180, 0.], # leg params: stiffness, l0, alpha, beta
'lp2' : [13000, 1, 65.5 * pi / 180, 0.],
}
IC = [0, .99, 0, .825, 0, 0.] # x,y,z, x',y',z'
frc = 550
class cfSLIP(bslip.BSLIP):
""" class derived from BSLIP
give a constant leg force
"""
def __init__(self,params=None, IC=None):
super(cfSLIP, self).__init__(params,IC)
self.dt = .01
def legfunc2(self, t, y, pars):
"""
leg function of leg 2: a spring function
:args:
t (float): time (ignored)
y (6x float): CoM state [position, velocity]
pars (dict): parameters of the model. Must include
'foot1' (3x float) foot1 position
'lp1' (4x float) parameters of leg 1
:returns:
f (float): the axial leg force ["f = k * (l - l0)"]
NOTE: Overwrite this function to get different models.
The signature must not change.
"""
l2 = norm(array(y[:3]) - array(pars['foot2']))
f = -pars['lp2'][0] * (l2 - pars['lp2'][1])
if f > 10:
return frc
else:
return f
def legfunc1(self, t, y, pars):
"""
leg function of leg 2: a spring function
:args:
t (float): time (ignored)
y (6x float): CoM state [position, velocity]
pars (dict): parameters of the model. Must include
'foot1' (3x float) foot1 position
'lp1' (4x float) parameters of leg 1
:returns:
f (float): the axial leg force ["f = k * (l - l0)"]
NOTE: Overwrite this function to get different models.
The signature must not change.
"""
l2 = norm(array(y[:3]) - array(pars['foot1']))
f = -pars['lp1'][0] * (l2 - pars['lp1'][1])
if f > 10:
return frc
else:
return f
b = cfSLIP(params=p, IC=IC)
for rep in range(4):
_ = b.do_step()
The init method calls
and stores the resulting visobjects in a list which will be updated.
The call method refers to update_frame
The init method gets
provides the update() method which updates the object according to given datapoints. Note: update takes keywords and maps to self.dict (!)
In [31]:
class visobject(object):
""" contains a visualizeable object """
def __init__(self, ax, data, props):
"""
Creates the visualizeable object with given properties, including
lines and patches.
:args:
ax (axes3D): the axes object to attach the drawing to
data (ndarray): data points describing the object.
props: set self.props to props (will be evaluated in draw)
"""
self.data = data
self.props = props
self.ax = ax
def update(self, **kwargs):
self.__dict__.update(kwargs)
def draw(self, frame):
""" must not be reached - must be overwritten """
raise NotImplementedError("Overwrite the draw method!")
class vline(visobject):
"""
creates a vertical dashed line on a 2D axis object
"""
def __init__(self, ax, data=None, props=None):
"""
initialize the vline object
:args:
ax: axes (2D) object
data (any): ingnored, for interface compatibility
props (any): ingnored, for interface compatibility
"""
super(vline, self).__init__(ax, data, props)
self.line = self.ax.plot([], [], 'k--')[0]
def draw(self, frame, aframe):
"""
updates the vertical dashed line position to animframe.sim_time
"""
self.line.set_data([aframe.sim_time, aframe.sim_time], self.ax.get_ylim())
def rotation_matrix(rotaxis, theta):
"""
returns a matrix that rotates vectors aroud "rotaxis" by theta.
Uses the Euler-Rodrigues formula.
This code was copied from stackoverflow.com
*Note:* This is comparatively slow. Checkout e.g. scipy.weave for
speedup (cf also sourceforce)
:args:
rotaxis (3x float): axis to rotate around
theta (float): angle in radians to rotate
returns:
rotmat [3x3 float]: rotation matrix
"""
rotaxis = rotaxis/norm(rotaxis)
a = cos(theta / 2.)
b, c, d = -rotaxis * sin(theta / 2.)
return array([[a*a+b*b-c*c-d*d, 2*(b*c-a*d), 2*(b*d+a*c)],
[2*(b*c+a*d), a*a+c*c-b*b-d*d, 2*(c*d-a*b)],
[2*(b*d-a*c), 2*(c*d+a*b), a*a+d*d-b*b-c*c]])
def get_2orth(vec):
"""
get two unit vectors orthogonal to vec
:args:
vec (3x float): vector to find orthogonal vectors to
:returns:
[v1, v2]: two vectors (3x float) which are orthogonal to vec.
*NOTE*
* This routine is robust in the sense that it works equally
well for all vectors vec.
* It is guaranteed that similar vectors yield similar
results (results are not unique!)
* It is also guaranteed that [vec, v1, v2] build a right-hand
coordinate system
"""
uvec = array(vec).squeeze() / norm(array(vec).squeeze())
# test if uvec and x-axis are aligned:
if abs(uvec[0]) > .9999:
# closely aligned to x axis
# basically, return [0, 1, 0] and [0, 0, 1] (slightly adjusted)
y2 = -cross(array([0, 0, 1]), uvec)
y2 /= norm(y2)
z2 = cross(y2, uvec) # should actually have unit norm
z2 /= norm(z2)
#elif uvec[0] < .9999:
# # closely aligned to (negative) x axis
# # basically, return [0, -1, 0] and [0, 0, 1] (slightly adjusted)
# y2 = array([0, 1, 0])
else:
# uvec and [1, 0, 0] span a plane. rotate y- and z axes accordingly.
#uperp: unit vector in plane, perpendicular to x-axis
xaxis = array([1, 0, 0])
uperp = uvec.copy()
uperp[0] = 0
uperp = uperp / norm(uperp)
rotaxis = cross(xaxis, uperp)
rotangle = arccos(uvec[0])
rm = rotation_matrix(rotaxis, rotangle)
y2 = dot(rm, [0, 1, 0])
z2 = dot(rm, [0, 0, 1])
return y2, z2
# alternatively, this could be nicely done using linear algebra.
# However, this does not yield a smooth function!
#proj = outer(uvec, uvec)
#u,s,v = svd(eye(3) - proj)
#if dot(cross(uvec, v[0,:]), v[1, :]) < 0:
# v[1, :] *= -1
#return v[0, :], v[1, :]
class SLIPleg_vis(visobject):
def __init__(self, ax, data, props):
"""
Creates the visualization of a SLIP leg.
:args:
ax: axes_3d object
data (list): Data has the following format:
data[frame][0] : hip position in 3D
data[frame][1] : toe position in 3D
props: (type to be defined) self.props (used by self.draw)
"""
super(SLIPleg_vis, self).__init__(ax, data, props)
phi = linspace(0,pi,60)
theta = linspace(0, 2*pi*6, 60)
r = .033
l0 = 1
self.raw_x = linspace(0, l0, 100)
self.raw_y = hstack([[0, ] * 20, sin(phi) * sin(theta) * r, [0, ] * 20])
self.raw_z = hstack([[0, ] * 20, sin(phi) * cos(theta) * r, [0, ] * 20])
self.color = 'r'
self.line = self.ax.plot([], [], [], color=self.color)[0]
#self.line = ax.plot_3D()
def draw(self, frame, aframe=None):
"""
Updates the SLIP leg position according to the data stored in self.data, at frame 'frame.
:args:
frame (integer): the frame number to plot
aframe (animframe object): ignored here
:returns:
self.line (Line3D): The line object (for passing e.g. to animations)
*NOTE* this does not perform the drawing process - call 'draw()' separately!
"""
hip = self.data[frame][0]
toe = self.data[frame][1]
x = self.raw_x
y = self.raw_y
z = self.raw_z
leg = array(hip) - array(toe)
v1 = array(leg).squeeze()
v2, v3 = get_2orth(leg)
tmat = vstack([v1, v2, v3]).T
data_3d = dot(tmat, vstack([self.raw_x, self.raw_y, self.raw_z])).T
data_3d += array(toe)
self.line.set_data(data_3d[:,0], data_3d[:,1])
self.line.set_3d_properties(data_3d[:, 2])
self.line.set_color(self.color)
return self.line
class CoM_vis(visobject):
def __init__(self, ax, data, props):
"""
Creates the visualization of a SLIP CoM.
:args:
ax: axes_3d object
data (list): Data has the following format:
data[frame]: CoM position in 3D
props: (type to be defined) self.props (used by self.draw)
*NOTE* because of a lack of my skills with matplotlib, this object
is RECREATED every frame!
"""
super(CoM_vis, self).__init__(ax, data, props)
#self.r = .05
u = linspace(0, 2 * pi, 100) # spherical coordinates
v = linspace(0, pi, 100,)
r = .06
self.x_pos_raw = r * outer(cos(u), sin(v))
self.y_pos_raw = r * outer(sin(u), sin(v))
self.z_pos_raw = r * outer(ones(size(u)), cos(v))
self.surf = self.ax.plot_surface(self.x_pos_raw, self.y_pos_raw, self.z_pos_raw,
rstride=5, cstride=5, color='r', shade=True, linewidth=0)
def draw(self, frame, aframe=None):
"""
Updates the CoM position according to the data stored in self.data, at frame 'frame'.
:args:
frame (integer): the frame number to plot
aframe (animframe object): ignored here
:returns:
self.line (Line3D): The line object (for passing e.g. to animations)
*NOTE* this does not perform the drawing process - call 'draw()' separately!
"""
xpos = self.x_pos_raw + self.data[frame][0]
ypos = self.y_pos_raw + self.data[frame][1]
zpos = self.z_pos_raw + self.data[frame][2]
self.surf.remove()
self.surf = self.ax.plot_surface(xpos, ypos, zpos,
rstride=5, cstride=5, color='r', shade=True, linewidth=0)
In [32]:
from mpl_toolkits.mplot3d.axes3d import Axes3D
class animframe(object):
"""
object that contains every element for the walking SLIP animation
"""
def __init__(self,figsize=(12,6.75)):
self.fig = figure(figsize=figsize)
self.ax_3d = self.fig.add_axes([0, 0, .8, 1], projection='3d')
self.ax_3d.view_init(10,25)
self.ax_3d.set_xlabel('x')
self.ax_3d.set_ylabel('y')
self.ax_3d.set_zlabel('z')
self.daxes = [] # collection of data axes
self.center = [0, 0] # center, e.g. CoM position
self.create_data_axes()
self.sim_time = 0
self.vobjects = []
draw()
def create_data_axes(self):
"""
creates all the nice axes at the right.
"""
self.daxes = []
ax = self.fig.add_axes([.8,0.6,.2,.4], frameon=False)
ax.set_title('forces of leg 1')
ax.set_xticks([])
ax.set_yticks([])
self.daxes.append(ax)
ax = self.fig.add_axes([.8,0.05,.2,.4], frameon=False)
ax.set_title('forces of leg 2')
ax.set_xticks([])
ax.set_yticks([])
self.daxes.append(ax)
def add(self, vobject):
"""
adds a visualizeable object
(e.g., a leg)
to the anim frame
"""
self.vobjects.append(vobject)
def draw(self, framenr, centerdata = None, timedata = None, **kwargs):
#self.ax_3d.cla()
if centerdata is not None:
self.center = centerdata[framenr]
if timedata is not None:
self.sim_time = timedata[framenr]
for elem in self.vobjects:
elem.draw(framenr, self)
self.ax_3d.set_xlim3d([self.center[0] - 1, self.center[0] + 1])
self.ax_3d.set_ylim3d([self.center[1] - 1, self.center[1] + 1])
self.ax_3d.set_zlim3d([0, 1.6])
for ax in self.daxes:
ax.set_xlim(self.sim_time - 1, self.sim_time + 1)
def elements(self):
return tuple(self.vobjects)
In [104]:
from pylab import pi
p = { 'foot1' : [0, 0, 0],
'foot2' : [-.5, 0, 0],
'm' : 80,
'g' : [0, -9.81, 0],
'lp1' : [13100, 1, 68.5 * pi / 180, -0.05], # leg params: stiffness, l0, alpha, beta
'lp2' : [12900, 1, 68.5 * pi / 180, 0.1],
}
IC = [0, .98, 0, 1.1, 0.01, 0.] # x,y,z, x',y',z'
reload(bslip)
c = bslip.BSLIP_newTD(p, IC)
# run the model as often as you want (repeat this cell!)
for rep in range(30):
_ = c.do_step()
for rep in range(30):
_ = c.do_step()
In [33]:
# map simulation data to "animation" data
# step 1: create continous CoM data
all_t = hstack([hstack([ts[1:], td[1:]]) for ts, td in zip (c.t_ss_seq, c.t_ds_seq)])
all_y = vstack([vstack([ys[1:, :], yd[1:, :]]) for ys, yd in zip (c.y_ss_seq, c.y_ds_seq)])
all_f = vstack([vstack([fs[1:, :], fd[1:, :]]) for fs, fd in zip (c.forces_ss_seq, c.forces_ds_seq)])
# step 2: create continous feet position data
footpos1 = zeros((all_t.shape[0], 3))
footpos2 = zeros((all_t.shape[0], 3))
k1, l01, alpha1, beta1 = c.params['lp1'][:4]
k2, l02, alpha2, beta2 = c.params['lp2'][:4]
idx = 0
#odd
odd = True # simulation start with leg1 on ground
for nr in arange(len(c.t_ss_seq)):
n_s = len(c.t_ss_seq[nr]) - 1 # number of frames for single-stance
n_d = len(c.t_ds_seq[nr]) - 1
if odd: # leg1 on ground at beginning
# single stance
footpos1[idx:idx + n_s, :] = c.feet1_seq[nr]
vx, vz = all_y[idx:idx + n_s, 3], all_y[idx:idx + n_s, 5]
a_v_com = -arctan2(vz, vx)
xf = all_y[idx:idx + n_s, 0] + l02 * cos(alpha2) * cos(beta2 + a_v_com)
yf = all_y[idx:idx + n_s, 1] - l02 * sin(alpha2)
zf = all_y[idx:idx + n_s, 2] - l02 * cos(alpha2) * sin(beta2 + a_v_com)
footpos2[idx:idx + n_s,: ] = vstack([xf, yf, zf]).T
# double stance
footpos1[idx + n_s:idx + n_s + n_d, :] = c.feet1_seq[nr]
footpos2[idx + n_s:idx + n_s + n_d, :] = c.feet2_seq[nr + 1]
odd = False
else:
# single stance of leg2
footpos2[idx:idx + n_s, :] = c.feet2_seq[nr]
vx, vz = all_y[idx:idx + n_s, 3], all_y[idx:idx + n_s, 5]
a_v_com = -arctan2(vz, vx)
xf = all_y[idx:idx + n_s, 0] + l01 * cos(alpha1) * cos(beta1 + a_v_com)
yf = all_y[idx:idx + n_s, 1] - l01 * sin(alpha1)
zf = all_y[idx:idx + n_s, 2] - l01 * cos(alpha1) * sin(beta1 + a_v_com)
footpos1[idx:idx + n_s,: ] = vstack([xf, yf, zf]).T
# double stance
footpos2[idx + n_s:idx + n_s + n_d, :] = c.feet2_seq[nr]
footpos1[idx + n_s:idx + n_s + n_d, :] = c.feet1_seq[nr + 1]
odd = True
idx += n_s + n_d
In [34]:
# linearly interpolate data
t_sim = arange(0, all_t[-1], .02)
y_sim = vstack([interp(t_sim, all_t, all_y[:, col]) for col in range(all_y.shape[1])]).T
f1_sim = vstack([interp(t_sim, all_t, footpos1[:, col]) for col in range(footpos1.shape[1])]).T
f2_sim = vstack([interp(t_sim, all_t, footpos2[:, col]) for col in range(footpos2.shape[1])]).T
forces_sim = vstack([interp(t_sim, all_t, all_f[:, col]) for col in range(all_f.shape[1])]).T
In [35]:
vis_coord = array([2,0,1])
# CoM and foot position data in a format required for the visobjects
c_data = y_sim[:,vis_coord]
f1_data = [ (y, f) for y, f in zip(y_sim[:, vis_coord], f1_sim[:, vis_coord])]
f2_data = [ (y, f) for y, f in zip(y_sim[:, vis_coord], f2_sim[:, vis_coord])]
a = animframe()
SL1 = SLIPleg_vis(a.ax_3d, f1_data, None)
SL2 = SLIPleg_vis(a.ax_3d, f2_data, None)
SL2.color='g'
CoM = CoM_vis(a.ax_3d, c_data, None)
a.add(SL1)
a.add(SL2)
a.add(CoM)
# create force plots
VL1 = vline(ax = a.daxes[0])
VL2 = vline(ax = a.daxes[1])
a.add(VL1)
a.add(VL2)
a.daxes[0].plot(t_sim, forces_sim[:, :3])
a.daxes[1].plot(t_sim, forces_sim[:, 3:])
#a.center = c_data[10]
#a.sim_time = t_sim[10]
# plot feet positions
for x,y,z in c.feet1_seq:
a.ax_3d.plot3D([z, ], [x, ], [0, ], 'rd')
for x,y,z in c.feet2_seq:
a.ax_3d.plot3D([z, ], [x, ], [0, ], 'gd')
a.draw(10, c_data, t_sim)
draw()
print a.daxes[0].get_xlim()
print a.daxes[1].get_xlim()
In [36]:
# do the animation!
from matplotlib import animation
anim = animation.FuncAnimation(a.fig, a.draw, frames=len(c_data), interval=20, fargs=(c_data, t_sim ))
# save the animation as an mp4. This requires ffmpeg or mencoder to be
# installed. The extra_args ensure that the x264 codec is used, so that
# the video can be embedded in html5. You may need to adjust this for
# your system: for more information, see
# http://matplotlib.sourceforge.net/api/animation_api.html
anim.save('walking_SLIP.mp4', fps=25, codec='libx264', clear_temp=True, ) #extra_args=['-vcodec', 'libx264']) # for other ipython versions
# to increase quality: set clear_temp to False, and:
# in a shell, do (e.g.):
#"avconv -y -r 20 -f image2 -i _tmp%04d.png -vb 600k bslip.ogg"
In [37]:
# display the video inline!
# note: this needs a recent browser... with HTML5 and the option of OGG-video playback
from IPython.core.display import HTML
video = open('walking_SLIP.ogg', 'rb').read()
video_encoded = video.encode('base64') # encode the video in HTML-conform style
video_tag = ('<video controls type="video/ogg" '
+ 'alt="VPP walking model" src="data:video/ogg;base64,{0}">').format(video_encoded)
HTML(data = video_tag)
In [24]:
# video_encoded
Out[24]:
In [ ]: